{
 "cells": [
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "a1921d43",
   "metadata": {},
   "outputs": [],
   "source": [
    "import math\n",
    "import sympy as sp\n",
    "import sympy  as sp\n",
    "from sympy.matrices import Matrix\n",
    "from functools import partial\n",
    "import numpy as np\n",
    "import matplotlib.pyplot as plt\n",
    "from mpl_toolkits import mplot3d\n",
    "from math import pi\n",
    "import pprint\n",
    "pp=pprint.PrettyPrinter(indent=5)\n",
    "\n",
    "theta_i, alpha_i, d_i, a_i, A_i, a_3, d_1, d_3, d_5, d_7 = sp.symbols('theta_i alpha_i d_i a_i A_i a_3 d_1, d_3, d_5, d_7')\n",
    "theta_1,theta_2,theta_3,theta_4,theta_5,theta_6,theta_7 = sp.symbols ('theta_1,theta_2, theta_3, theta_4, theta_5, theta_6, theta_7')\n",
    "Rot_z = sp.Matrix([ [sp.cos(theta_i), -sp.sin(theta_i),0,0], [sp.sin(theta_i),sp.cos(theta_i),0,0], [0,0,1,0], [0,0,0,1] ]);\n",
    "Rot_x = sp.Matrix([ [1,0,0,0], [0,sp.cos(alpha_i), -sp.sin(alpha_i),0], [0, sp.sin(alpha_i), sp.cos(alpha_i), 0], [0,0,0,1] ]); \n",
    "Tran_z = sp.Matrix([[1,0,0,0], [0,1,0,0], [0,0,1,d_i], [0,0,0,1]]);\n",
    "Tran_x = sp.Matrix([[1,0,0,a_i], [0,1,0,0], [0,0,1,0], [0,0,0,1]]);\n",
    "\n",
    "A_i=Rot_z*Tran_z*Tran_x*Rot_x;\n",
    "\n",
    "# Back Left Leg\n",
    "\n",
    "Ad0=A_i.subs([(theta_i,math.radians(90)),(alpha_i,math.radians(90)),(a_i,0),(d_i,0)])\n",
    "A1=A_i.subs([(theta_i,theta_1),(alpha_i,math.radians(0)),(a_i,0),(d_i,0)])\n",
    "Ad1=A_i.subs([(theta_i,math.radians(-90)),(alpha_i,math.radians(90)),(a_i,0),(d_i,64.85)])\n",
    "A2=A_i.subs([(theta_i,theta_2),(alpha_i,math.radians(0)),(a_i,0),(d_i,-140)])\n",
    "Ad2=A_i.subs([(theta_i,math.radians(0)),(alpha_i,math.radians(0)),(a_i,500),(d_i,0)])\n",
    "A3=A_i.subs([(theta_i,theta_3),(alpha_i,math.radians(0)),(a_i,0),(d_i,0)])\n",
    "A4=A_i.subs([(theta_i,0),(alpha_i,math.radians(0)),(a_i,450),(d_i,0)])\n",
    "\n",
    "# Back Right Leg\n",
    "Ad0=A_i.subs([(theta_i,math.radians(-90)),(alpha_i,math.radians(90)),(a_i,0),(d_i,0)])\n",
    "A1=A_i.subs([(theta_i,theta_1),(alpha_i,math.radians(0)),(a_i,0),(d_i,0)])\n",
    "Ad1=A_i.subs([(theta_i,math.radians(-90)),(alpha_i,math.radians(-90)),(a_i,0),(d_i,64.85)])\n",
    "A2=A_i.subs([(theta_i,theta_2),(alpha_i,math.radians(0)),(a_i,0),(d_i,140)])\n",
    "Ad2=A_i.subs([(theta_i,math.radians(0)),(alpha_i,math.radians(0)),(a_i,500),(d_i,0)])\n",
    "A3=A_i.subs([(theta_i,theta_3),(alpha_i,math.radians(0)),(a_i,0),(d_i,0)])\n",
    "A4=A_i.subs([(theta_i,0),(alpha_i,math.radians(0)),(a_i,450),(d_i,0)])\n",
    "\n",
    "\n",
    "#Front Left Leg\n",
    "Ad0=A_i.subs([(theta_i,math.radians(90)),(alpha_i,math.radians(90)),(a_i,0),(d_i,0)])\n",
    "A1=A_i.subs([(theta_i,theta_1),(alpha_i,math.radians(0)),(a_i,0),(d_i,0)])\n",
    "Ad1=A_i.subs([(theta_i,math.radians(-90)),(alpha_i,math.radians(90)),(a_i,0),(d_i,64.85)])\n",
    "A2=A_i.subs([(theta_i,theta_2),(alpha_i,math.radians(0)),(a_i,0),(d_i,-140)])\n",
    "Ad2=A_i.subs([(theta_i,math.radians(0)),(alpha_i,math.radians(0)),(a_i,500),(d_i,0)])\n",
    "A3=A_i.subs([(theta_i,theta_3),(alpha_i,math.radians(0)),(a_i,0),(d_i,0)])\n",
    "A4=A_i.subs([(theta_i,0),(alpha_i,math.radians(0)),(a_i,450),(d_i,0)])\n",
    "\n",
    "\n",
    "#Front Right Leg\n",
    "Ad0=A_i.subs([(theta_i,math.radians(-90)),(alpha_i,math.radians(90)),(a_i,0),(d_i,0)])\n",
    "A1=A_i.subs([(theta_i,theta_1),(alpha_i,math.radians(0)),(a_i,0),(d_i,0)])\n",
    "Ad1=A_i.subs([(theta_i,math.radians(-90)),(alpha_i,math.radians(-90)),(a_i,0),(d_i,-64.85)])\n",
    "A2=A_i.subs([(theta_i,theta_2),(alpha_i,math.radians(0)),(a_i,0),(d_i,140)])\n",
    "Ad2=A_i.subs([(theta_i,math.radians(0)),(alpha_i,math.radians(0)),(a_i,500),(d_i,0)])\n",
    "A3=A_i.subs([(theta_i,theta_3),(alpha_i,math.radians(0)),(a_i,0),(d_i,0)])\n",
    "A4=A_i.subs([(theta_i,0),(alpha_i,math.radians(0)),(a_i,450),(d_i,0)])\n",
    "\n",
    "\n",
    "T1=Ad0*A1\n",
    "T2=Ad0*A1*Ad1*A2\n",
    "T3=Ad0*A1*Ad1*A2*Ad2*A3\n",
    "T4=Ad0*A1*Ad1*A2*Ad2*A3*A4\n",
    "\n",
    "Z0 = T1[:3,2]\n",
    "Z1 = T2[:3,2]\n",
    "Z2 = T3[:3,2]\n",
    "Z3 = T4[:3,2]\n",
    "Xp=T4[:3,3]\n",
    "diff_thet_1 = Xp.diff(theta_1) #Partially differentiating Xp wrt θ1\n",
    "diff_thet_2 = Xp.diff(theta_2) #Partially differentiating Xp wrt θ2\n",
    "diff_thet_3 = Xp.diff(theta_3) #Partially differentiating Xp wrt θ4\n",
    "J = Matrix([[diff_thet_1[0],diff_thet_2[0],diff_thet_3[0]],\n",
    "          [diff_thet_1[1],diff_thet_2[1],diff_thet_3[1]],\n",
    "          [diff_thet_1[2],diff_thet_2[2],diff_thet_3[2]],\n",
    "          [Z0[0],Z1[0],Z2[0]],[Z0[1],Z1[1],Z2[1]],[Z0[2],Z1[2],Z2[2]]])\n",
    "\n",
    "\n",
    "x,y,z,r,o=sp.symbols(\"x y z r theta\")\n",
    "dt=0.05  #Time difference\n",
    "t_1=[math.radians(0)]  #Theta 1\n",
    "t_2=[-0.7]  #Theta 2\n",
    "t_3=[1.5]  #Theta 4\n",
    "\n",
    "T=T4\n",
    "x_tool=[]\n",
    "y_tool=[]\n",
    "z_tool=[]\n",
    "#Tool Velocity Matrix\n",
    "X=sp.Matrix([[0],[100*sp.sin((2*np.pi*o)/200)*(2*np.pi)/10],[100*sp.cos((2*np.pi*o)/200)*(2*np.pi)/10],[0],[0],[0]])\n",
    "i=0\n",
    "print(\"Computing Trajectory\")\n",
    "while i<=100:\n",
    "    X_eval=X.subs(o,i)\n",
    "    T_eval=T.subs([(theta_1,t_1[i]),(theta_2,t_2[i]),(theta_3,t_3[i])])\n",
    "    x_tool.append(T_eval[3])\n",
    "    y_tool.append(T_eval[7])\n",
    "    z_tool.append(T_eval[11])\n",
    "    J_eval=J.subs([(theta_1,t_1[i]),(theta_2,t_2[i]),(theta_3,t_3[i])])\n",
    "    q=J_eval.pinv()*X_eval\n",
    "    q=q*dt\n",
    "#     print(q)\n",
    "    t_1.append(q[0]+t_1[i])\n",
    "    t_2.append(q[1]+t_2[i])\n",
    "    t_3.append(q[2]+t_3[i])\n",
    "    print(\".\",end=\"\")\n",
    "    i=i+1\n",
    "\n",
    "plt.plot(y_tool,z_tool)\n",
    "# plt.scatter(0,0.725)\n",
    "plt.xlabel(\"y coordinate\")\n",
    "plt.ylabel(\"z coordinate\")\n",
    "plt.axis(\"equal\")\n",
    "plt.title(\"Trajectory of Robot\")\n",
    "plt.grid(True)\n",
    "plt.show()"
   ]
  }
 ],
 "metadata": {
  "kernelspec": {
   "display_name": "Python 3 (ipykernel)",
   "language": "python",
   "name": "python3"
  },
  "language_info": {
   "codemirror_mode": {
    "name": "ipython",
    "version": 3
   },
   "file_extension": ".py",
   "mimetype": "text/x-python",
   "name": "python",
   "nbconvert_exporter": "python",
   "pygments_lexer": "ipython3",
   "version": "3.9.0"
  }
 },
 "nbformat": 4,
 "nbformat_minor": 5
}
